/********************************************************************************

 **** Copyright (C), 2018, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
 * File Name     : FocControlFunction.c
 * Author        : Bruce HW&RD
 * Date          : 2018-12-27
 * Description   : .C file function description
 * Version       : 1.0
 * Function List :
 *
 * Record        :
 * 1.Date        : 2018-12-27
 *   Author      : Bruce HW&RD
 *   Modification: Created file

********************************************************************************/

#include <FU68xx_4.h>
#include <MyProject.h>

CurrentOffset xdata mcCurOffset;
/*---------------------------------------------------------------------------*/
/* Name     :   void FOC_Init(void)
/* Input    :   NO
/* Output   :   NO
/* Description: mcInit状态下，对FOC的相关寄存器进行配置,先清理寄存器，后配置，最后使能
/*---------------------------------------------------------------------------*/
void FOC_Init(void)
{
    /*使能FOC*/
    ClrBit(DRV_CR, FOCEN);
    SetBit(DRV_CR, FOCEN);
    SetBit(FOC_CR0, ESCMS);
    /*----------------------------------------------------------------------
      MAX ERR配置
      00：0.5  01：0.25  10：0.125  11：1.0
    ----------------------------------------------------------------------*/
    SetBit(FOC_CR0, UCSEL);
    //    ClrBit(FOC_CR0 , MERRS1);
    //    SetBit(FOC_CR0 , MERRS0);
    FOC_EOMEKLPF    = 0xff;
    FOC_KFG         = 0;
    /*配置FOC寄存器*/
    FOC_CR1         = 0;                                    // 清零 FOC_CR1
    FOC_CR2         = 0;                                    // 清零 FOC_CR2
    FOC_IDREF       = 0;                                    // 清零 Id
    FOC_IQREF       = 0;                                    // 清零 Iq
    FOC__THETA      = 0;                                    // 清零 角度
    FOC_RTHEACC     = 0;                                    // 清零 爬坡函数的初始加速度
    FOC__RTHESTEP   = 0;                                    // 清零 爬坡速度
    FOC__RTHECNT    = 0;                                    // 清零 爬坡次数
    #if(Open_Start_Mode ==Omega_Start)
    {
        FOC__THECOMP    =  _Q15(-15.0 / 180.0);                    // SMO 估算补偿角
    }
    #else
    {
        FOC__THECOMP    = _Q15(-5.0 / 180.0);                    // SMO 估算补偿角
    }
    #endif
    FOC__THECOR     = 0x04;                                 // 误差角度补偿
    /*电流环参数配置*/
    FOC_DMAX        = DOUTMAX;
    FOC_DMIN        = DOUTMIN;
    FOC_QMAX        = QOUTMAX;
    FOC_QMIN        = QOUTMIN_Start;//QOUTMIN;
    /*位置估算参数配置*/
    FOC_EK1         = OBS_K1T;
    FOC_EK2         = OBS_K2T;
    FOC_EK3         = OBS_K3T;
    FOC_EK4         = OBS_K4T;
    FOC_FBASE       = OBS_FBASE;
    FOC_OMEKLPF     = SPEED_KLPF;
    FOC_EBMFK       = OBS_KLPF;
    /*********PLL或SMO**********/
    #if (EstimateAlgorithm == SMO)
    {
        ClrBit(FOC_CR2, ESEL);
        FOC_KSLIDE    = OBS_KSLIDE;
        FOC_EKLPFMIN  = OBS_EA_KS;
    }
    #elif (EstimateAlgorithm == PLL)
    {
        SetBit(FOC_CR2, ESEL);
        FOC_KSLIDE    = OBSE_PLLKP_GAIN;
        FOC_EKLPFMIN  = OBSE_PLLKI_GAIN;
    }
    #endif //end SVPMW_Mode
    SetBit(FOC_CR1, SVPWMEN);                            // SVPWM模式
    
    /*正反转*/
    if (mcFRState.FR == 0)                               // 设置F/R
    {
        SetBit(DRV_CR, DDIR);
    }
    else
    {
        ClrBit(DRV_CR, DDIR);
    }
    
    #if (IRMODE == 1)
    {
        SetBit(DRV_CR, DDIR);                            // 反转标志位
    }
    #endif //end IRMODE
    /**过调制**/
    #if (OverModulation == 1)
    {
        SetBit(FOC_CR1, OVMDL);                            // 过调制
    }
    #endif //end OverModulation
    /*单电阻采样；需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式*/
    #if (Shunt_Resistor_Mode == Single_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
        FOC_TSMIN  = PWM_TS_LOAD;                  // 最小采样窗口
        FOC_TRGDLY = 0x0D;                         // 采样时刻在中点，一般考虑开关噪声影响，会设置延迟；
        // 0x0c表示延迟12个clock，提前用反码形式，如0x84表示提前12个clock。
        ClrBit(FOC_CR2, F5SEG);                    // 7段式
        SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
    }
    /*双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia，配置81*/
    #elif (Shunt_Resistor_Mode == Double_Resistor) // double resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
        FOC_TSMIN = PWM_DT_LOAD;                   // 死区补偿值
        FOC_TRGDLY = 0x06;                         // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同
        // 01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_DLOWL_TIME;                 //下桥臂最小脉冲，保证采样
        SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
        /*五段式或七段式选择*/
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                  // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                  // 5段式
        }
        #endif
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS);                    // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS);                    // 5段式
        }
        #endif //end DouRes_Sample_Mode
    }
    /*三电阻采样*/
    #elif (Shunt_Resistor_Mode == Three_Resistor)   // signel resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0 | CSM1); // 三电阻
        FOC_TSMIN  = PWM_DT_LOAD; // 死区补偿值
        FOC_TRGDLY = 0x06;        // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同。
        // 01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, CMP3MOD0 | CMP3MOD1);
        FOC_TBLO = PWM_OVERMODULE_TIME;             // 过调制电流采样处理的TB脉宽
        /*五段式或七段式选择*/
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                             // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                             // 5段式
        }
        #endif //end SVPMW_Mode
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS);                               // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS);                               // 5段式
        }
        #endif //end DouRes_Sample_Mode
    }
    #endif  //end Shunt_Resistor_Mode
    
    /* 使能电流基准校正 */
    if (mcCurOffset.OffsetFlag == 1)
    {
        #if (Shunt_Resistor_Mode == Single_Resistor)          // 单电阻校正
        {
            /*set ibus current sample offset*/
            SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
            FOC_CSO = mcCurOffset.Iw_busOffset;               // 写入Ibus的偏置
        }
        #elif (Shunt_Resistor_Mode == Double_Resistor)        // 双电阻校正
        {
            /*set ia, ib current sample offset*/
            SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
            FOC_CSO  = mcCurOffset.IuOffset;                  // 写入IA的偏置
        
            SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
            FOC_CSO  = mcCurOffset.IvOffset;                  // 写入IB的偏置
        }
        #elif (Shunt_Resistor_Mode == Three_Resistor)         // 三电阻校正
        {
            /*set ibus current sample offset*/
            SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
            FOC_CSO = mcCurOffset.IuOffset;                   // 写入IA的偏置
        
            SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
            FOC_CSO = mcCurOffset.IvOffset;                   // 写入IB的偏置
        
            SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
            FOC_CSO = mcCurOffset.Iw_busOffset;               // 写入IC的偏置
        }
        #endif  //end Shunt_Resistor_Mode
    }
    
    /*-------------------------------------------------------------------------------------------------
    DRV_CTL：PWM来源选择
    OCS = 0, DRV_COMR
    OCS = 1, FOC/SVPWM/SPWM
    -------------------------------------------------------------------------------------------------*/
    /*计数器比较值来源FOC*/
    SetBit(DRV_CR, OCS);
}
/*---------------------------------------------------------------------------*/
/* Name     :   void Motor_Charge(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 预充电，当一直处于预充电状态下，不接电机，可用于验证IPM或者Mos。
预充电分三步，第一步是对U相进行预充电，第二步是对U,V两相进行预充电;第三步是对U、V、W三相进行预充电。
/*---------------------------------------------------------------------------*/
void Motor_Charge(void)
{
    if (McStaSet.SetFlag.ChargeSetFlag == 0)
    {
        McStaSet.SetFlag.ChargeSetFlag = 1;
        DRV_DR = 0.1 * DRV_ARR;              //下桥臂10% duty
        /*-------------------------------------------------------------------------------------------------
        DRV_CTL：PWM来源选择
        OCS = 0, DRV_COMR
        OCS = 1, FOC/SVPWM/SPWM
        -------------------------------------------------------------------------------------------------*/
        ClrBit(DRV_CR, OCS);
        mcFocCtrl.ChargeStep = 0;
    }
    
    if ((mcFocCtrl.State_Count < Charge_Time) && (mcFocCtrl.ChargeStep == 0))
    {
        mcFocCtrl.ChargeStep = 1;
        DRV_CMR |= 0x03;                    // U相下桥臂通
        MOE = 1;
    }
    
    if (( mcFocCtrl.State_Count <= (Charge_Time << 1) / 3) && (mcFocCtrl.ChargeStep == 1))
    {
        mcFocCtrl.ChargeStep = 2;
        DRV_CMR = 0x0F;                     // V相下桥臂导通
    }
    
    if ((mcFocCtrl.State_Count <= Charge_Time / 3) && (mcFocCtrl.ChargeStep == 2))
    {
        mcFocCtrl.ChargeStep = 3;
        DRV_CMR = 0x3F;                     // W相下桥臂导通
    }
}
/*---------------------------------------------------------------------------*/
/* Name     :   void Motor_Align(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 预定位函数，当无逆风判断时，采用预定位固定初始位置;当有逆风判断时，采用预定位刹车
/*---------------------------------------------------------------------------*/
void Motor_Align(void)
{
    if (McStaSet.SetFlag.AlignSetFlag == 0)
    {
        DRV_ARR = (uint16)(MCU_CLOCK * 1000 * 2 / PWM_FREQUENCY) - 1;      // 载波频率的周期值
        /*设置DRV计数器的比较匹配值，当DRV计数值与COMR相等时，根据DRV_SR寄存器的DCIM是否产生比较匹配事件*/
        DRV_COMR = ((uint16)(MCU_CLOCK * 1000 * 2 / PWM_FREQUENCY) >> 3);
        McStaSet.SetFlag.AlignSetFlag = 1;
        /*FOC初始化*/
        FOC_Init();
        /*配置预定位的电流、KP、KI*/
        FOC_IDREF = ID_Align_CURRENT;
        FOC_IQREF = IQ_Align_CURRENT;
        FOC_DQKP  = DQKP_Alignment;
        FOC_DQKI  = DQKI_Alignment;
        FOC_EKP   = OBSW_KP_GAIN;
        FOC_EKI   = OBSW_KI_GAIN;
        /*配置预定位角度*/
        #if (AlignTestMode == 1)
        FOC__THETA      = Align_Theta;
        #else
        #if (PosCheckEnable == 1)
        FOC__THETA  = mcFocCtrl.mcPosCheckAngle + _Q15((60.0) / 180.0);
        #else
        FOC__THETA  = 0;//Align_Theta;
        #endif
        #endif
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        FOC__ETHETA   = FOC__THETA - 4096;
        #elif (EstimateAlgorithm == PLL)
        FOC__ETHETA   = FOC__THETA;
        #endif //end
        /*使能输出*/
        DRV_CMR |= 0x3F;                         // U、V、W相输出
        MOE = 1;
    }
}

/*---------------------------------------------------------------------------*/
/* Name     :   void Motor_Open(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 开环启动的参数配置
/*---------------------------------------------------------------------------*/
void Motor_Open(void)
{
    static uint8 OpenRampCycles = 0;
    
    if (McStaSet.SetFlag.StartSetFlag == 0)
    {
        McStaSet.SetFlag.StartSetFlag = 1;
        FOC_Init();
        FOC__UD = 0;
        FOC__UQ = _Q15(0.02);                       //Uq给一个初始值，防止IQ给定瞬间电流太大
        ClrBit(FOC_CR2, UDD);
        ClrBit(FOC_CR2, UQD);
        /****启动初始角度赋值****/
        #if (PosCheckEnable==1)                      //初始位置检测不使能时初始角度为预定位角度
        FOC__THETA  = RPDPara.ThetaGet;              // 无初始位置检测，则用预定位角
        FOC__ETHETA = FOC__THETA ;
        #elif (PosCheckEnable==0)                    //初始位置检测不使能时初始角度为预定位角度
        FOC__THETA  = Align_Theta;                   // 无初始位置检测，则用预定位角
        #endif
        /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
        FOC_IDREF = ID_Start_CURRENT;                         // D轴启动电流
        mcFocCtrl.mcIqref = IQ_Start_CURRENT;                 // Q轴启动电流
        FOC_DQKP = DQKPStart;
        FOC_DQKI = DQKIStart;
        FOC_EKP  = OBSW_KP_GAIN;
        FOC_EKI  = OBSW_KI_GAIN;
        /*启动方式选择*/
        #if (Open_Start_Mode == Omega_Start)              // Omega 启动
        {
            FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
            FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
            FOC_EFREQHOLD = Motor_Omega_Ramp_End;
            SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
        }
        #elif (Open_Start_Mode == Open_Start)
        {
            FOC_RTHEACC      = Motor_Open_Ramp_ACC;      // 爬坡函数的初始加速度
            FOC__RTHESTEP    = Motor_Open_Ramp_Min;      // 0.62 degree acce speed
            FOC_RTHECNT      = MOTOR_OPEN_ACC_CNT;       // acce time
            SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
        }
        #elif (Open_Start_Mode == Open_Low_Theta_Start)
        {
            LowSpeedCtrl.LPF_Ratio = 60;
            LowSpeedCtrl.FREQ_Theta = 10;  //一个载波对应角度值，如果小于这个值就一直强拖
            LowSpeedCtrl.Swicth_Theta_DEC = 40;//5个载波对应角度值，如果小于这个值就一直强拖
            LowSpeedCtrl.Motor_Open_ACC = Motor_Open_Ramp_ACC;
            LowSpeedCtrl.Motor_Open_Speed_Min = Motor_Open_Ramp_Min;
            LowSpeedCtrl.Motor_Open_CNT  = MOTOR_OPEN_ACC_CNT;
            Open_Low_Start_Init();
            FOC__RTHECNT  = 1;
        }
        #elif (Open_Start_Mode == Open_Omega_Start)
        {
            FOC_RTHEACC   = Motor_Open_Ramp_ACC;         // 爬坡函数的初始加速度
            FOC__RTHESTEP = Motor_Open_Ramp_Min;         // 0.62 degree acce speed
            FOC__RTHECNT  = MOTOR_OPEN_ACC_CNT;          // acce time
            FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
            FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
            FOC_EFREQHOLD = Motor_Omega_Ramp_End;
            SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | RFAE | ANGM);
        }
        #endif //end Open_Start_Mode
    }
    
    /*不同启动方式下，切换到MCRUN状态*/
    #if (Open_Start_Mode == Open_Start)         //OPEN状态启动时拖动多次
    {
        if (OpenRampCycles < (MOTOR_OPEN_ACC_CYCLE - 1))
        {
            if (!ReadBit(FOC_CR1, RFAE))
            {
                SetBit(FOC_CR1, RFAE);
                OpenRampCycles++;
            }
        }
        else
        {
            mcFocCtrl.State_Count = 2;
            mcState = mcRun;
            SetBit(FOC_CR1, ANGM);
        }
        
        FOC_EKP = OBSW_KP_GAIN_RUN4;                            // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN4;                            // 估算器里的PI的KI
    }
    #elif (Open_Start_Mode == Open_Low_Theta_Start)
    {
        if (!ReadBit(FOC_CR1, RFAE))
        {
            FOC__RTHECNT  = LowSpeedCtrl.Motor_Open_CNT;
            mcFocCtrl.State_Count = 1680;
            SetBit(FOC_CR1, RFAE);
            OpenRampCycles++;
            mcState = mcRun;
            LowSpeedCtrl.LowSpeedFlag = 0;
        }
    }
    #elif (Open_Start_Mode == Open_Omega_Start)
    {
        mcFocCtrl.State_Count = 2600;
        mcState = mcRun;
    }
    #elif (Open_Start_Mode == Omega_Start)
    {
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        {
            mcFocCtrl.State_Count = 1680;
        }
        #elif (EstimateAlgorithm == PLL)
        {
            mcFocCtrl.State_Count = 1680;
            FOC_EKP = OBSW_KP_GAIN_RUN4;                       // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4;                       // 估算器里的PI的KI
        }
        #endif //end    EstimateAlgorithm
        mcState   = mcRun;
    }
    #endif //end Open_Start_Mode
    FOC_IQREF = mcFocCtrl.mcIqref;                          // Q轴启动电流
}

/*---------------------------------------------------------------------------*/
/* Name     :   void Motor_Align(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 顺风逆风参数配置函数
/*---------------------------------------------------------------------------*/
void Motor_TailWind(void)
{
    if (mcFocCtrl.State_Count == 0)
    {
        if (McStaSet.SetFlag.TailWindSetFlag == 0) //初始化
        {
            McStaSet.SetFlag.TailWindSetFlag = 1;
            #if (FRDetectMethod == RSDMethod)
            RSDDetectInit();
            #elif (FRDetectMethod == BEMFMethod)
            BEMFDetectInit();
            #elif (FRDetectMethod == FOCMethod)
            TailWindDetectInit();
            #endif
        }
        
        #if (FRDetectMethod == RSDMethod)
        RSDDealwith();
        #elif (FRDetectMethod == BEMFMethod)
        BEMFDealwith();
        #elif (FRDetectMethod == FOCMethod)
        FOCTailWindDealwith();
        #endif
    }
}

/*---------------------------------------------------------------------------*/
/* Name     :   void MC_Stop(void)
/* Input    :   NO
/* Output   :   NO
/* Description: inital motor control parameter
/*---------------------------------------------------------------------------*/
void MC_Stop(void)
{
    MOE     = 0;
    ClrBit(DRV_CR, FOCEN);    //关闭FOC                                                                                                         // disable FOC output and initial register
    mcState = mcInit;
}

/*-------------------------------------------------------------------------------------------------
    Function Name : void MotorControlInit(void)
    Description   : 控制变量初始化清零,包括保护参数的初始化、电机状态初始化
    Input         : 输入说明（详细）
  Output                :   输出说明（详细）
-------------------------------------------------------------------------------------------------*/
void MotorcontrolInit(void)
{
    /* -----启动控制变量初始化----- */
    memset(&LowSpeedCtrl, 0, sizeof(LOWSPEEDCONCTROL));
    memset(&mcFaultDect, 0, sizeof(FaultVarible));              // 保护变量清零
    memset(&mcProtectTime, 0, sizeof(ProtectVarible));          // 保护次数清零
    memset(&mcCurVarible, 0, sizeof(CurrentVarible));           // 电流保护的变量清零
    memset(&mcFocCtrl, 0, sizeof(FOCCTRL));                     // 外部控制环变量清零
    memset(&AdcSampleValue, 0, sizeof(ADCSample));              // ADCSample变量清零
    memset(&Time, 0, sizeof(TimeCnt));                          // 时间计数初始化
    memset(&mcSpeedRamp, 0, sizeof(MCRAMP));                    // mcSpeedRamp变量清零
    //    memset(&SleepSet , 0 , sizeof(SLEEPMODE));                  // 睡眠模式清零
    /*****顺逆风判断，FOCMethod不需要在此赋初值*******/
    #if (FRDetectMethod == RSDMethod)
    {
        memset(&RSDDetect, 0, sizeof(MotorRSDTypeDef));         //  ISDDetect所有变量清零
    }
    #elif (FRDetectMethod == BEMFMethod)
    {
        memset(&BEMFDetect, 0, sizeof(BEMFDetect_TypeDef));     //  BEMFDetect所有变量清零
    }
    #endif
    McStaSet.SetMode  = 0;                                      // 电机状态机时序变量
    mcFRState.TargetFR = 0;                                     // 电机目标方向
    /**************************电流偏置校准变量**********************/
    memset(&mcCurOffset, 0, sizeof(CurrentOffset));             // mcCurOffset变量清零
    mcCurOffset.IuOffsetSum            = 16383;
    mcCurOffset.IvOffsetSum            = 16383;
    mcCurOffset.Iw_busOffsetSum        = 16383;
    /*PWM调速变量*/
    //    memset(&mcPwmInput , 0 , sizeof(PWMINPUTCAL));              // mcPwmInput变量清零
    /* Flash Security key init */
    FlashKey.x5a = 0x5a;
    FlashKey.x1f = 0x1f;
}

/*---------------------------------------------------------------------------*/
/* Name     :   void VariablesPreInit(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 初始化电机参数
/*---------------------------------------------------------------------------*/
void VariablesPreInit(void)
{
    /* -----启动控制变量初始化----- */
    memset(&LowSpeedCtrl, 0, sizeof(LOWSPEEDCONCTROL));
    /***********保护******************/
    mcFaultSource = 0;
    memset(&mcFaultDect, 0, sizeof(FaultVarible));                                                                 // FaultVarible变量清零
    /*****外部控制环*******/
    memset(&mcFocCtrl, 0, sizeof(FOCCTRL));         // mcFocCtrl变量清零
    mcFocCtrl.OUTLoopCtrl.Close_loop_delay_time = SpeedloopDeley_Time;
    mcFocCtrl.OUTLoopCtrl.PIControlPeriod = SPEED_LOOP_TIME;
    /*****顺逆风判断*******/
    #if (FRDetectMethod == RSDMethod)
    {
        RSDDetect.RSDCCWTimes    = 0;       //  CCW刹车次数清零
    }
    #elif (FRDetectMethod == BEMFMethod)
    {
    }
    #elif (FRDetectMethod == FOCMethod)
    {
        memset(&TailWindDetect, 0, sizeof(MotorTailWindTypeDef));   //  TailWindDetect所有变量清零
    }
    #endif
    memset(&RPDPara, 0, sizeof(RPD_Param_TypeDef));
    RPDPara.NextShotFlag = 1;
    RPDPara.InjectOffFocIntCnt = Inject_Off_Foc_Int_Cnt;
    RPDPara.InjectOnFocIntCnt = Inject_On_Foc_Int_Cnt;
    RPDPara.InjectStep1VoltageProportion = Inject_Step1_Voltage_Proportion;
    RPDPara.InjectStep2VoltageProportion = Inject_Step2_Voltage_Proportion;
    /*****电机状态机时序变量***********/
    McStaSet.SetMode                  = 0x01;   //电流校准标志位置1，其它置0
    TailWindDetect.count = 0;
    TailWindDetect.CWFlage = 0;
}

/*---------------------------------------------------------------------------*/
/* Name     :   void GetCurrentOffset(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
                调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，OffsetFlag置1。
/*---------------------------------------------------------------------------*/
void GetCurrentOffset(void)
{
    if (!mcCurOffset.OffsetFlag)
    {
        SetBit(ADC_CR, ADCBSY);             // 使能ADC
        
        while (ReadBit(ADC_CR, ADCBSY));
        
        #if (Shunt_Resistor_Mode == Single_Resistor)                   //单电阻模式
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
        #elif (Shunt_Resistor_Mode == Double_Resistor)                 //双电阻模式
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
        #elif (Shunt_Resistor_Mode == Three_Resistor)                 //三电阻模式
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
        #endif
        mcCurOffset.OffsetCount++;
        
        if (mcCurOffset.OffsetCount > Calib_Time)
        {
            mcCurOffset.OffsetFlag = 1;
        }
    }
}
/*---------------------------------------------------------------------------*/
/* Name     :   void Motor_Ready(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 上电时，关闭输出，先对硬件电路的电流进行采集，在FOC_Init中写入对应的校准寄存器中。
                调试时，需观察mcCurOffset结构体中对应变量是否在范围内。
/*---------------------------------------------------------------------------*/
void Motor_Ready(void)
{
    if (McStaSet.SetFlag.CalibFlag == 0)
    {
        McStaSet.SetFlag.CalibFlag = 1;
        ClrBit(DRV_CR, FOCEN);
        ClrBit(DRV_CR, DRVEN);
        MOE = 0;
        SetBit(ADC_MASK, CH4EN | CH2EN | CH1EN | CH0EN);
        //   mcCurOffset.OffsetFlag = 0;
    }
}
/*---------------------------------------------------------------------------*/
/* Name     :   void Motor_Init(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 对电机相关变量、PI进行初始化设置
/*---------------------------------------------------------------------------*/
void Motor_Init(void)
{
    ClrBit(ADC_MASK, CH1EN | CH0EN);      // 关闭软件电流采样的ADC
    VariablesPreInit();                           // 电机相关变量初始化
    PI_Init();                                    // PI初始化
    SetBit(DRV_CR, DRVEN);
}

#if 0
void FRControl(void)
{
    //正反转切换，存在如下情况：
    //1、关机或错误状态下正反转切换，则直接改变当前旋转状态mcFRState.FR，下一次启动则生效
    //2、运行或启动状态，先关机并标记该状态，关机后进入Break状态直接改变旋转状态，
    //且标记时重新开机并恢复转速
    //3、正反转过程时关机，则保持关机状态，反转/正转切换后状态下一次开机后生效
    if (IRControl.FlagFR)
    {
//        if (mcFocCtrl.SpeedFlt > (MOTOR_FR_SPEED_LEVEL_7 + 50) )
//        {
//            mcFRState.OldFRTarSpeed = mcFRState.OldTargetSpeed;
//            IRControl.TargetSpeed = 7;
//            IRControl.FlagSpeed = 1;
//        }
//        else
//        {
            if ((mcState == mcReady) || (mcState == mcFault))
            {
                //改变当前转向
                if (mcFRState.FR == CW)
                {
                    mcFRState.FR = CCW;
                }
                else
                {
                    mcFRState.FR = CW;
                }
                
                //若为运行时正反转切换则恢复到切换前的转速并开机
                if (mcFRState.FlagFR)
                {
                    mcState = mcInit;
                    IRControl.ONOFFStatus = 1;
                    mcSpeedRamp.FlagONOFF = 1;
                    //IRControl.TargetSpeed = mcFRState.OldTargetSpeed;
                    IRControl.TargetSpeed = mcFRState.OldFRTarSpeed;
                    IRControl.FlagSpeed = 1;
                    mcFRState.FlagFR = 0;
                }
                
                IRControl.FlagFR = 0;
            }
            else if ((mcState == mcRun) || (mcState == mcStart))
            {
                //控制关机
                mcSpeedRamp.FlagONOFF = 0;
                IRControl.TargetSpeed = 0;
                IRControl.FlagSpeed = 1;
                //运行状态下正反转切换置位标记
                mcFRState.FlagFR = 1;
            }
//        }
    }
}
#endif

/*  -------------------------------------------------------------------------------------------------
    Function Name  : EstimateSwitch
    Description    : 估算器切换
    Date           : 2020-12-21
    Parameter      : value:
    ------------------------------------------------------------------------------------------------- */
#if 0
void EstimateSwitch(void)
{
    int ETHETATemp = 0;
    ETHETATemp = FOC__ETHETA;
    
    if ((mcFocCtrl.SpeedFlt > Motor_Loop_Speed) //&& (mcFocCtrl.SpeedFlt > abs(LowSpeedCtrl.LowSpeed) * 0.80)
        && (LowSpeedCtrl.LowSpeedFlag == 0))
    {
        LowSpeedCtrl.Low_Swicth_Time = 1000;
        FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
        FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
        FOC__EOME     = mcFocCtrl.SpeedFlt;
        FOC_EFREQHOLD = mcFocCtrl.SpeedFlt;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
        LowSpeedCtrl.LowSpeedFlag = 1;
    }
    else if ((LowSpeedCtrl.LowSpeedFlag == 1) || (LowSpeedCtrl.Low_Swicth_Time > 0))
    {
        //        if ((mcFocCtrl.SpeedFlt < (Motor_Loop_Speed - Motor_Loop_Speed/4 ))
        //            || (mcFocCtrl.SpeedFlt > 3 * mcFocCtrl.EsValue))
        //        {
        //            FOC_DQKP = DQKPStart;
        //            FOC_DQKI = DQKIStart;
        //            LowSpeedCtrl.LowSpeedFlag = 0;
        //            mcFocCtrl.CtrlMode = 0;
        //            FOC_IQREF          = IQ_RUN_CURRENT;
        //            FOC__EOME          =  0;
        //            FOC_EFREQMIN       =  0;
        //            mcFocCtrl.SpeedFlt =  0;
        //            SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
        //            ClrBit(FOC_CR1, EFAE);
        //            ClrBit(FOC_CR1, ANGM);
        //            LowSpeedCtrl.LowSpeedFlag = 0;
        //        }
    }
    
    if ((LowSpeedCtrl.Low_Swicth_Time > 0))
    {
        mcSpeedRamp.ActualValue = Motor_Loop_Speed;
        FOC__THECOR = 0x1F;
        FOC_DQKP = DQKP;//;Start;
        FOC_DQKI = DQKI;//Start;
        mcFocCtrl.State_Count = 100;
        LowSpeedCtrl.Low_Swicth_Time--;
    }
}
#endif
